#ifndef CONTROLFSM_H
#define CONTROLFSM_H

#include <iostream>
#include <memory>
// Contains all of the control related data
#include "ControlFSMData.h"

// Checks the robot state and commands for safety
#include "SafetyChecker.h"

// FSM States
#include "../FSM_States/FSM_State.h"
#include "../FSM_States/FSM_State_BalanceStand.h"
#include "../FSM_States/FSM_State_ImpedanceControl.h"
#include "../FSM_States/FSM_State_JointPD.h"
#include "../FSM_States/FSM_State_Locomotion.h"
#include "../FSM_States/FSM_State_Passive.h"
#include "../FSM_States/FSM_State_StandUp.h"
#include "../FSM_States/FSM_State_RecoveryStand.h"
#include "../FSM_States/FSM_State_Vision.h"
#include "../FSM_States/FSM_State_BackFlip.h"
#include "../FSM_States/FSM_State_FrontJump.h"

#define FSM_Q 0

/**
 * Enumerate all of the operating modes
 */
enum class FSM_OperatingMode { 
    NORMAL, TRANSITIONING, ESTOP, EDAMP };

/**
 *
 */
template <typename T>
struct FSM_StatesList
{
    FSM_State<T>* invalid;
    FSM_State_Passive<T>* passive;
    FSM_State_JointPD<T>* jointPD;
    FSM_State_ImpedanceControl<T>* impedanceControl;
    FSM_State_StandUp<T>* standUp;
    FSM_State_BalanceStand<T>* balanceStand;
    FSM_State_Locomotion<T>* locomotion;
    FSM_State_RecoveryStand<T>* recoveryStand;
    FSM_State_Vision<T>* vision;
    FSM_State_BackFlip<T>* backflip;
    FSM_State_FrontJump<T>* frontJump;
};

template <typename T>
class FSM_StateList_Q
{
public:
    FSM_StateList_Q(ControlFSMData<T>& data)
    {
        this->_passive = new FSM_State_Passive<T>(&data);
        this->_jointPD = new FSM_State_JointPD<T>(&data);
        this->_impedanceControl = new FSM_State_ImpedanceControl<T>(&data);
        this->_standUp = new FSM_State_StandUp<T>(&data);
        this->_balanceStand = new FSM_State_BalanceStand<T>(&data);
        this->_locomotion = new FSM_State_Locomotion<T>(&data);
        this->_recoveryStand = new FSM_State_RecoveryStand<T>(&data);
        this->_vision = new FSM_State_Vision<T>(&data);
        this->_backflip = new FSM_State_BackFlip<T>(&data);
        this->_frontJump = new FSM_State_FrontJump<T>(&data);
    }

    FSM_StateList_Q(){}

    ~FSM_StateList_Q()
    {
        if (this->_passive)
            delete this->_passive;

        if (this->_jointPD)
            delete this->_jointPD;

        if (this->_impedanceControl)
            delete this->_impedanceControl;

        if (this->_standUp)
            delete this->_standUp;

        if (this->_balanceStand)
            delete this->_balanceStand;

        if (this->_locomotion)
            delete this->_locomotion;

        if (this->_recoveryStand)
            delete this->_recoveryStand;

        if (this->_vision)
            delete this->_vision;

        if (this->_backflip)
            delete this->_backflip;

        if (this->_frontJump)
            delete this->_frontJump;
    }

    FSM_State<T>* invalid()
    {
        return nullptr;
    }

    FSM_State<T>* passive()
    {
        return this->_passive;
    }

    FSM_State<T>* jointPD()
    {
        return this->_jointPD;
    }

    FSM_State<T>* impedanceControl()
    {
        return this->_impedanceControl;
    }

    FSM_State<T>* standUp()
    {
        return this->_standUp;
    }

    FSM_State<T>* balanceStand()
    {
        return this->_balanceStand;
    }

    FSM_State<T>* locomotion()
    {
        return this->_locomotion;
    }

    FSM_State<T>* recoveryStand()
    {
        return this->_recoveryStand;
    }

    FSM_State<T>* vision()
    {
        return this->_vision;
    }

    FSM_State<T>* backflip()
    {
        return this->_backflip;
    }

    FSM_State<T>* frontJump()
    {
        return this->_frontJump;
    }
private:
    //FSM_State<T>* _invalid{nullptr};
    FSM_State_Passive<T>* _passive{nullptr};
    FSM_State_JointPD<T>* _jointPD{nullptr};
    FSM_State_ImpedanceControl<T>* _impedanceControl{nullptr};
    FSM_State_StandUp<T>* _standUp{nullptr};
    FSM_State_BalanceStand<T>* _balanceStand{nullptr};
    FSM_State_Locomotion<T>* _locomotion{nullptr};
    FSM_State_RecoveryStand<T>* _recoveryStand{nullptr};
    FSM_State_Vision<T>* _vision{nullptr};
    FSM_State_BackFlip<T>* _backflip{nullptr};
    FSM_State_FrontJump<T>* _frontJump{nullptr};
};

/**
 *
 */
template <typename T>
struct FSM_ControllerList {
};

/**
 * Control FSM handles the FSM states from a higher level
 */
template <typename T>
class ControlFSM
{
public:
    ControlFSM(Quadruped<T>* _quadruped,
               StateEstimatorContainer<T>* _stateEstimator,
               LegController<T>* _legController, GaitScheduler<T>* _gaitScheduler,
               DesiredStateCommand<T>* _desiredStateCommand,
               RobotControlParameters* controlParameters,
               VisualizationData* visualizationData,
               MIT_UserParameters* userParameters);

    virtual ~ControlFSM();

    // Initializes the Control FSM instance
    void initialize();

    // Runs the FSM logic and handles the state transitions and normal runs
    void runFSM();

    // This will be removed and put into the SafetyChecker class
    FSM_OperatingMode safetyPreCheck();

    //
    FSM_OperatingMode safetyPostCheck();

    // Gets the next FSM_State from the list of created states when requested
    FSM_State<T>* getNextState(FSM_StateName stateName);

    // Prints the current FSM status
    void printInfo(int opt);

    // Contains all of the control related data
    ControlFSMData<T> data;

    // FSM state information
#if FSM_Q
    FSM_StatesList<T> statesList;  // holds all of the FSM States
#else
    std::shared_ptr<FSM_StateList_Q<T>> statesList{nullptr};
#endif
    FSM_State<T>* currentState;    // current FSM state
    FSM_State<T>* nextState;       // next FSM state
    FSM_StateName nextStateName;   // next FSM state name

    // Checks all of the inputs and commands for safety
    SafetyChecker<T>* safetyChecker;

    TransitionData<T> transitionData;

private:
    // Operating mode of the FSM
    FSM_OperatingMode operatingMode;

    // Choose how often to print info, every N iterations
    int printNum = 10000;  // N*(0.001s) in simulation time

    // Track the number of iterations since last info print
    int printIter = 0;  // make larger than printNum to not print

    int iter = 0;

    //lcm::LCM state_estimator_lcm;
    //state_estimator_lcmt _state_estimator;
};

#endif  // CONTROLFSM_H
